// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SimpleCartesianMoveAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime.Arm
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Collision;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    using ShoNS.Array;

    /// <summary>
    /// A class that listens to Cartesian position control message,
    /// does simple motion planning, and then uses joint position
    /// controller to execute the trajectory.
    /// This class is supposed to sit above the level of direct
    /// hardware control.
    /// </summary>
    [DataContract]
    public abstract class SimpleCartesianMoveAgent :
        ConsumerAgent<ManipulatorStatusMessage>,
        IControlReceiver<CartesianGoalPositionControlMessage>
    {
        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext moveAgentContext = TraceContexts.FromString(TraceContexts.Manip, "MoveAgent");

        /// <summary>
        /// The move parameter object
        /// </summary>
        [DataMember(Name = "MoveParameter")]
        private SimpleCartesianMoveParameter moveParameter;

        /// <summary>
        /// The name of the cubic spline trajectory
        /// </summary>
        [DataMember(Name = "trajectoryControlAgentName")]
        private string trajectoryControlAgentName;

        /// <summary>
        /// Name of the visualizer agent
        /// </summary>
        [DataMember]
        private string visualizerAgentName;

        /// <summary>
        /// The most recent CartesianGoalMotionControlMessage that is not executed
        /// </summary>
        private CartesianGoalPositionControlMessage mostRecentPendingCartesianMotionControlMessage;

        /// <summary>
        /// The trajectory control agent
        /// </summary>
        private IAgentHost trajectoryControlAgent;

        /// <summary>
        /// The trajectory control port
        /// </summary>
        private IForwarder<StartTrajectoryMessage> trajectoryControlPort;

        /// <summary>
        /// Arm status
        /// </summary>
        private ManipulatorStatus armStatus;

        /// <summary>
        /// The visualizer agent
        /// </summary>
        private IAgentHost visualizerAgent;

        /// <summary>
        /// Initializes a new instance of the <see cref="SimpleCartesianMoveAgent" /> class.
        /// </summary>
        /// <param name="name">The name of the agent</param>
        /// <param name="producer">The producer which generates arm status</param>
        /// <param name="trajectoryControlAgent">The trajectory control agent which
        /// is in charge of executing a trajectory</param>
        /// <param name="kc">Arm kinematic chain</param>
        /// <param name="defaultIKSeeds">Default IK seeds</param>
        /// <param name="jointTolerance">Joint move tolerance</param>
        /// <param name="minimumJointHeight">Minimum height to avoid collision</param>
        /// <param name="maximumJointVel">Maximum joint velocity</param>
        /// <param name="cutOffPercentageForIKFiltering">The percentage of the top score for IK filtering</param>
        /// <param name="minimumTrajectoryTime">The minimum time for executing a trajectory</param>
        /// <param name="guaranteedClearance">The guaranteed clearance below the end effector</param>
        /// <param name="minimumElbowHeight">Minimum elbow height from table surface</param>
        /// <param name="trajectoryQualityThreshold">The threshold for trajectory quality</param>
        /// <param name="jointWeightFactors">Joint weight factors</param>
        /// <param name="elbowClearanceCheckList">Elbow clearance checklist</param>
        /// <param name="visualizerAgentName">Name of the visualizer</param>
        public SimpleCartesianMoveAgent(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            IControllable<StartTrajectoryMessage> trajectoryControlAgent,
            KinematicChain kc,
            List<double[]> defaultIKSeeds,
            double jointTolerance,
            double minimumJointHeight,
            double maximumJointVel,
            double cutOffPercentageForIKFiltering,
            double minimumTrajectoryTime,
            double guaranteedClearance,
            double minimumElbowHeight,
            double trajectoryQualityThreshold,
            double[] jointWeightFactors,
            bool[] elbowClearanceCheckList,
            string visualizerAgentName = null)
            : base(name, producer.Name)
        {
            this.trajectoryControlAgentName = trajectoryControlAgent.Name;
            this.moveParameter = new SimpleCartesianMoveParameter(
                kc,
                defaultIKSeeds,
                jointTolerance,
                minimumJointHeight,
                maximumJointVel,
                cutOffPercentageForIKFiltering,
                minimumTrajectoryTime,
                guaranteedClearance,
                minimumElbowHeight,
                trajectoryQualityThreshold,
                jointWeightFactors,
                elbowClearanceCheckList);

            this.visualizerAgentName = visualizerAgentName;

            this.mostRecentPendingCartesianMotionControlMessage = null;
            this.armStatus = null;
        }

        /// <summary>
        /// Called when the agent is initialized
        /// </summary>
        /// <param name="locator">Agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.trajectoryControlAgent = this.Locator.GetAgent(this.trajectoryControlAgentName);
            this.trajectoryControlPort = this.trajectoryControlAgent.GetControlPort<StartTrajectoryMessage>();

            if (null != this.visualizerAgentName)
            {
                this.visualizerAgent = this.Locator.GetAgent(this.visualizerAgentName);
            }

            TraceOut.Info(moveAgentContext, "SimpleCartesianMoveAgent is now initialized.");
            System.Console.WriteLine("SimpleCartesianMoveAgent is now initialized.");
        }

        /// <summary>
        /// Receive arm status message
        /// </summary>
        /// <param name="msg">Message to process</param>
        public override void Receive(ManipulatorStatusMessage msg)
        {
            bool inCollision = KinematicChainSelfCollisionChecker.CheckSelfCollision(this.moveParameter.ArmKinematicChain, DoubleArray.From(msg.State.JointPositions).Transpose());

            if (true == inCollision)
            {
                System.Console.WriteLine("In collision {0}", msg.OriginatingTime);
            }

            // check if there was a control message received before, execute it if there was a control message
            // received before
            if (null == this.armStatus && null != this.mostRecentPendingCartesianMotionControlMessage)
            {
                this.armStatus = msg.State;
                this.ExecuteCartesianMotionControlMessage(this.mostRecentPendingCartesianMotionControlMessage);
            }
            else
            {
                this.armStatus = msg.State;
            }
        }

        /// <summary>
        /// Receive and handle arm Cartesian control message
        /// </summary>
        /// <param name="message">Message to process</param>
        public void ReceiveControl(CartesianGoalPositionControlMessage message)
        {
            if (null == this.armStatus)
            {
                TraceOut.Info(moveAgentContext, "No arm status messages received. Motion command is ignored but recorded.  Motion will be executed as soon as arm status is available.");

                // record this control message because it cannot be process now
                this.mostRecentPendingCartesianMotionControlMessage = message;
            }
            else
            {
                this.ExecuteCartesianMotionControlMessage(message);
            }
        }

        /// <summary>
        /// Gets the IK solution extender
        /// </summary>
        /// <param name="jointValues">Joint values</param>
        /// <returns>Equivalent IK solutions</returns>
        public abstract List<DoubleArray> InverseKinematicsExtender(DoubleArray jointValues);

        /// <summary>
        /// Execute a CartesianGoalMotionControlMessage
        /// </summary>
        /// <param name="message">A control message</param>
        private void ExecuteCartesianMotionControlMessage(CartesianGoalPositionControlMessage message)
        {
            // user information, when tracing is turned off, we want to inform the user the arm will be in motion soon
            // TODO: remove for production code - Bug 2121
            System.Console.WriteLine("ArmMove: Pos {0:F3} {1:F3} {2:F3}", message.CartesianPosition.Position.X, message.CartesianPosition.Position.Y, message.CartesianPosition.Position.Z);
            System.Console.WriteLine("ArmMove: Orient {0:F3} {1:F3} {2:F3} {3:F3}", message.CartesianPosition.Orientation.W, message.CartesianPosition.Orientation.X, message.CartesianPosition.Orientation.Y, message.CartesianPosition.Orientation.Z);

            // tracing information
            TraceOut.Info(moveAgentContext, "ArmMove: Pos {0} {1} {2}", message.CartesianPosition.Position.X, message.CartesianPosition.Position.Y, message.CartesianPosition.Position.Z);
            TraceOut.Info(moveAgentContext, "ArmMove: Orient {0} {1} {2} {3}", message.CartesianPosition.Orientation.W, message.CartesianPosition.Orientation.X, message.CartesianPosition.Orientation.Y, message.CartesianPosition.Orientation.Z);

            // IK calculation
            // try current joint angles as the seed for the IK problem
            DoubleArray currentJoint = DoubleArray.From(this.armStatus.JointPositions).Transpose();

            // TODO: This assumes there is at least one cm buffering between the gripper and the table at the target pose, still working on a better solution
            // This will be configurable after we have decided the final design.
            double minimumJointHeight = Math.Min(this.moveParameter.MinimumJointHeightFromTable, Math.Min(this.armStatus.TcpCartesianPosition.Position.Z, message.CartesianPosition.Position.Z)) - this.moveParameter.GuaranteedSafeClearanceBelowEE;

            DoubleArray solution = FindIKWithGoodElbowClearanceFromBaseXYPlane.FindIK(
                    this.moveParameter.ArmKinematicChain,
                    message.CartesianPosition,
                    currentJoint,
                    this.moveParameter.DefaultIKSeeds,
                    minimumJointHeight,
                    this.moveParameter.CutOffPercentageForIKSolutions,
                    DoubleArray.From(this.moveParameter.JointWeightFactors).Transpose(),
                    this.moveParameter.ElbowClearanceCheckList,
                    this.InverseKinematicsExtender,
                    message.TranslationalTolerance,
                    message.RotationalTolerance,
                    this.moveParameter.MinimumElbowHeight,
                    this.moveParameter.TrajectoryQualityThreshold,
                    0,
                    100);

            if (null != solution)
            {
                double maxJointTravel = Math.Max(Math.Abs((currentJoint - solution).Min()), (currentJoint - solution).Max());
                double duration = Math.Max(maxJointTravel / this.moveParameter.MaximumJointVelocity, this.moveParameter.MinimumTrajectoryTime);

                double[,] targetPositions = new double[solution.size0, 2];
                double[,] targetVelocities = new double[solution.size0, 2];
                double[] times = { 0, duration };

                // use solution value
                for (int i = 0; i < solution.size0; ++i)
                {
                    targetPositions[i, 1] = solution[i];
                }

                StartTrajectoryMessage trajMessage = new StartTrajectoryMessage(
                    AgentMessage.GetCurrentTime(),
                    solution.size0,
                    2,
                    times,
                    targetPositions,
                    targetVelocities,
                    TrajectoryInterpolationMethods.CubicSpline,
                    true);

                this.trajectoryControlPort.Post(trajMessage);

                // TODO: remove for production code - Bug 2121
                System.Console.WriteLine("Trajectory sent to controller");
            }
            else
            {
                TraceOut.Info(moveAgentContext, "No valid IK solutions found and no collision-free trajectory found for this movement");
                System.Console.WriteLine("No valid IK solutions found and no collision-free trajectory found for this movement");
            }
        }
    }
}
